#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S4, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ir,             sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     sonar,          sensorSONAR)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     frontLeft,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rearLeft,      tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     rearRight,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S4_C1_2,     frontRight,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S4_C2_1,     lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    guiderDrop,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    rightHook,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    pivot,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    spinner,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include <JoystickDriver.c>

int driveEncoderCycle = 1120;
float autoDriveScale = 0.7777;
//int pivotTarget = 1000;
//const int pivotIncrement = 1;

//void setPivot(float pivotSetting)
//{
//	servo [pivot] = 255 * pivotSetting;
//}

void drive(int x1, int y1, int x2)
{
	motor [frontLeft] = (y1 + x2 + x1);
	motor [frontRight] = (y1 - x2 - x1);
	motor [rearLeft] = (y1 + x2 - x1);
	motor [rearRight] = (y1 - x2 + x1);
}

void allStop()
{
	drive(0, 0, 0);
}

int resetDriveEncoders()
{
	nMotorEncoder [frontLeft] = 0;
	nMotorEncoder [frontRight] = 0;
	nMotorEncoder [rearLeft] = 0;
	nMotorEncoder [rearRight] = 0;
	return 1;
}

void initializeRobot()
{
	bDisplayDiagnostics = false;
	nxtDisplayCenteredTextLine(3, "Initializaing");
	nxtDisplayCenteredTextLine(4, "servos...");
	servo [pivot] = 0;
	servo [spinner] = 128;
	servo [guiderDrop] = 255;
	servo [leftHook] = 22;
	servo [rightHook] = 209;
	wait1Msec(1000);
	eraseDisplay();
	if (resetDriveEncoders() == 1)
	{
		nxtDisplayCenteredTextLine(3, "Encoders");
		nxtDisplayCenteredTextLine(4, "successfully");
		nxtDisplayCenteredTextLine(5, "reset.");
		wait1Msec(1000);
	}
	eraseDisplay();
	bDisplayDiagnostics = true;
}

task main ()
{
	initializeRobot();
	waitForStart();
#if 1
	ClearTimer(T1);
	while (time1[T1] < 800)
	{
		motor [arm] = -100;
	}
	motor [arm] = 0;
	wait1Msec(1000);
	while (nMotorEncoder [frontLeft] < driveEncoderCycle * 5.75)
	{
		drive(0, 60, 0);
	}
	allStop();
	wait1Msec (1000);
	while(nMotorEncoder [frontLeft] < driveEncoderCycle * 2.75)
	{
		drive(35, 0, 0);
	}
	allStop();
	#endif

}
